/**
 * @file ArmorDetector.cpp
 * @author 刘晨光 (Ermaotie@163.com)
 * @brief 装甲板检测
 * @version 0.1
 * @date 2021-09-30
 * 
 * @copyright Copyright (c) 2021
 * 
 */

#include "ArmorDetector.h"

float PI = 3.1415926535f;

/**
 * @brief 程序运行
 * 
 */
void ArmorDetector::run()
{
    VideoCapture cap = VideoCapture(video_path);
    while (cap.read(src))
    {
        vector<Edge> edges;

        split(src, channels);
        r_channel = channels[2]; // 提取 R通道
        threshold(r_channel, threshold_mask, thresh, 255, THRESH_BINARY);
        findContours(threshold_mask, contours, hierarchy, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);

        // 筛选光条
        for (auto &contour : contours)
        {
            RotatedRect min_rect = minAreaRect(contour);

            /**
             * @brief 规整化 RotatedRect 规整结果，以竖直线上半段为极轴，顺时针为正，逆时针为负，[-90,90）
             * 
             */
            float width = min_rect.size.width;
            float height = min_rect.size.height;
            float angle = min_rect.angle;

            /**
             * @brief 以高宽比以及面积大小筛选光条
             * 
             */
            RotatedRect reg_rect = angle < 45 ? min_rect : RotatedRect(min_rect.center, Size2f(height, width), -90 + angle);
            if ((reg_rect.size.height / reg_rect.size.width > lower_light_ratio) && (reg_rect.size.height / reg_rect.size.width < upper_light_ratio) && min_rect.size.area() > lower_light_area && min_rect.size.area() < upper_light_area)
            {
                Point point_edge_up = getSpecificPoint(reg_rect.center, reg_rect.angle, reg_rect.size.height * 0.5f);
                Point point_edge_down = getSpecificPoint(reg_rect.center, reg_rect.angle, -reg_rect.size.height * 0.5f);
                Edge edge;
                edge.up_point = point_edge_up;
                edge.down_point = point_edge_down;
                edge.center = reg_rect.center;
                edges.push_back(edge);
            }
        }
        /**
         * @brief 按照x坐标排序灯条
         * 
         */
        sort(edges.begin(), edges.end(),
             [&](const Edge &a, const Edge &b)
             {
                 return a.center.x < b.center.x;
             });

        /**
         * @brief 判断甲板并绘制在src上
         * 
         */
        if (edges.size() > 1)
        {
            for (int i = 0; i < edges.size() - 1; i++)
            {
                if (judgeArmor(edges[i], edges[i + 1], upper_armor_ratio, lower_armor_ratio))
                {
                    drawArmor(src, edges[i], edges[i + 1], line_color, point_color);
                }
            }
        }

        namedWindow("Result", WINDOW_AUTOSIZE);
        imshow("Result", src);
        waitKey(delay);
    }
};

/**
 * @brief 判断是否为甲板
 * 
 * @param a 边A
 * @param b 边B
 * @param upper_limit 高宽比上限
 * @param lower_limit 高宽比下限
 * @return true 
 * @return false 
 */
bool ArmorDetector::judgeArmor(const Edge &a, const Edge &b, const float upper_limit, const float lower_limit)
{
    float height_1 = getDistance(a.up_point, a.down_point);
    float height_2 = getDistance(b.up_point, b.down_point);
    float height = (height_1 + height_2) / 2;
    float width = getDistance(a.center, b.center);
    float ratio = height / width;
    return ratio < upper_limit && ratio > lower_limit;
}

/**
 * @brief 画出甲板位置，对角线及中心
 * 
 * @param out 
 * @param a 边A
 * @param b 边B
 * @param line_color 
 * @param point_color 
 */
void ArmorDetector::drawArmor(const Mat &out, const Edge &a, const Edge &b, Scalar line_color, Scalar point_color)
{
    line(out, a.up_point, b.down_point, line_color);
    line(out, a.down_point, b.up_point, line_color);
    Point2f center = getArmorCenter(a.up_point, a.down_point, b.up_point, b.down_point);
    circle(out, center, 2, point_color, 2);
}

/**
 * @brief Construct a new Armor Detector:: Armor Detector object
 * 
 * @param config_path config.yaml 文件路径 ，导入初始设定
 */
ArmorDetector::ArmorDetector(String config_path)
{
    FileStorage fread(config_path, FileStorage::READ);

    video_path = (String)fread["video_path"];
    thresh = (int)fread["thresh"];
    line_color = Scalar((int)fread["line_color"][0], (int)fread["line_color"][1], (int)fread["line_color"][2]);
    point_color = Scalar((int)fread["point_color"][0], (int)fread["point_color"][1], (int)fread["point_color"][2]);
    upper_light_ratio = (float)fread["upper_light_ratio"];
    lower_light_ratio = (float)fread["lower_light_ratio"];
    upper_light_area = (float)fread["upper_light_area"];
    lower_light_area = (float)fread["lower_light_area"];
    upper_armor_ratio = (float)fread["upper_armor_ratio"];
    lower_armor_ratio = (float)fread["lower_armor_ratio"];
    delay = (int)fread["delay"];
};